/**
 * @file ZigzagPlanner.h
 * @author  
 * @brief 遍历清洁规划
 * @version 1.0
 * @date 2020-07-23
 * 
 * @copyright Copyright (c) 2020
 * 
 */

#pragma once
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "Communication/NavigateToPoseActionClient.h"
#include <lib/ipa_room_planning.h>
#include "sweeping_robot_msg/srv/clean_next.hpp"

namespace behavior_controller
{
    
    class ZigzagPlanner
    {
        public :
            ZigzagPlanner() {;};

            void SetNode(const rclcpp::Node::SharedPtr &ros_node,std::shared_ptr<IpaRoomPlanning> &ipa_room_plan_);
            bool Plan();
            void run(int &finished_id,double &linear_vel, double &angular_vel,geometry_msgs::msg::PoseStamped &goal_pose);
            int  roomExploration(int room_id_);
            bool addRobotPose(std::vector<geometry_msgs::msg::PoseStamped> &poses, std::vector<geometry_msgs::msg::PoseStamped> &poses_sub);
            bool addRooms(std::vector<std::vector<cv::Point> > & contours);
            bool hasRepairLeak();
            std::shared_ptr<IpaRoomPlanning> & IPA_PLAN_PTR() {return ipa_room_plan;};
        private:
            void cleanCallback(
                const std::shared_ptr<rmw_request_id_t>/*request_header*/,
                const std::shared_ptr<sweeping_robot_msg::srv::CleanNext::Request>/*req*/,
                std::shared_ptr<sweeping_robot_msg::srv::CleanNext::Response>/*res*/);
            bool finishSection();
            bool checkDistance(geometry_msgs::msg::PoseStamped &check_pose, double dis_sqr= -1);

        private:    
            rclcpp::Service<sweeping_robot_msg::srv::CleanNext>::SharedPtr clean_service_=nullptr;
            std::shared_ptr<IpaRoomPlanning> ipa_room_plan=nullptr;
            rclcpp::Node::SharedPtr ros_node_;
            std::shared_ptr<sweeping_robot_msg::srv::CleanNext::Request> request=nullptr;
            geometry_msgs::msg::PoseStamped  last_pose,last_goal_pose;
            std::vector<geometry_msgs::msg::PoseStamped> plan_poses_ori,plan_poses_set;

            std::vector<int> optimal_room_sequence;
            std::vector<int> cut_off_ids;
            double time_now=0;
            int max_room=-1;
            int section_id=-1,max_section_id=-1;
            int point_id=-1,max_point_id=-1;
            bool room_ready=true;
            bool section_ready=true;
            bool done_prepare=false;

    };
    //static 
    extern ZigzagPlanner zigzag_planner; //实例化自检类


}